﻿// 2019/4/18: 首个记录
// 2019/9/19: 增加USoundSensorProperty默认构造函数
// 2020/1/14: 接口完备性
// 2022/11/3: 使用SetTime复制全部时间信息
// 2023/10/11: 补充部分英文注释
// 2025/3/31: 实现ToGeneralSample和Interpolate方法

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using ASEva;

namespace USoundSampleV1
{
    class USoundSensorProperty
    {
        public double OriginX { get; set; } // [m] X-coordinate of the n-th sensor's position / 传感器原点X坐标
        public double OriginY { get; set; } // [m] Y-coordinate of the n-th sensor's position / 传感器原点Y坐标
        public double Orientation { get; set; } // [°] Central heading angle of the n-th sensor / 传感器中心轴朝向
        public double AngleRangeAlpha { get; set; } // [°] α angle range (generally around 100°, with sensor's origin as the center of the circle) / α角范围（一般在100°左右，以OriginX/Y为圆心）
        public double AngleRangeBeta { get; set; } // [°] β angle range (generally ranges 0~30°) / β角范围（一般在0~30°）
        public double DistanceRangeAlpha { get; set; } // [m] Detect range corresponding to α angle (shorter) / α角对应的探测距离（较短）
        public double DistanceRangeBeta { get; set; } // [m] Detect range corresponding to β angle (longer) / β角对应的探测距离（较长）

        public USoundSensorProperty()
        {
            AngleRangeAlpha = 100;
            AngleRangeBeta = 15;
            DistanceRangeAlpha = 0.5;
            DistanceRangeBeta = 2.5;
        }
    }

    class USoundSensorData
    {
        public int ID { get; set; } // Sensor ID, starting from 1 / 传感器ID，从1开始
        public USoundSensorProperty Properties { get; set; } // Sensor properties / 超声波传感器属性
        public double? Distance { get; set; } // [m] Detected barrier's distance, null indicates no barrier / 障碍物的距离，无障碍物则为空

        public USoundSensorData()
        {
            Properties = new USoundSensorProperty();
        }
    }

    class USoundSample : Sample
    {
        public List<USoundSensorData> DataList { get; set; } // Data of each sensor / 各个超声波传感器的数据

        public USoundSample()
        {
            DataList = new List<USoundSensorData>();
        }

        public static string Title
        {
            get
            {
                return "Ultrasound sensor count,First sensor's ID,First sensor's detected distance[m],First sensor's origin-X[m],First sensor's origin-Y[m],First sensor's orientation[°],First sensor's α angle range[°],First sensor's β angle range[°],First sensor's α distance range[m],First sensor's β distance range[m],Second sensor's ID,etc";
            }
        }

        public static string Protocol
        {
            get
            {
                return "usound-sample-v1";
            }
        }

        public static string[] Protocols
        {
            get
            {
                return new string[] { "usound-sample-v1" };
            }
        }

        public override string GetGeneralSampleProtocol()
        {
            return Protocol;
        }

        public override string[] GetGeneralSampleProtocols()
        {
            return Protocols;
        }

        public override GeneralSample ToGeneralSample()
        {
            var sample = new GeneralSample();
            sample.SetTime(this);
            sample.Protocol = Protocol;

            var data = new GeneralSampleValue[1 + 9 * DataList.Count];
            sample.NumberOfSignificants = data.Length;

            data[0] = new GeneralSampleValue((double)DataList.Count);

            int baseIndex = 1;
            for (int i = 0; i < DataList.Count; i++)
            {
                var sensor = DataList[i];
                data[baseIndex + 0] = new GeneralSampleValue((double)sensor.ID);
                if (sensor.Distance != null) data[baseIndex + 1] = new GeneralSampleValue(sensor.Distance.Value);
                data[baseIndex + 2] = new GeneralSampleValue(sensor.Properties.OriginX);
                data[baseIndex + 3] = new GeneralSampleValue(sensor.Properties.OriginY);
                data[baseIndex + 4] = new GeneralSampleValue(sensor.Properties.Orientation);
                data[baseIndex + 5] = new GeneralSampleValue(sensor.Properties.AngleRangeAlpha);
                data[baseIndex + 6] = new GeneralSampleValue(sensor.Properties.AngleRangeBeta);
                data[baseIndex + 7] = new GeneralSampleValue(sensor.Properties.DistanceRangeAlpha);
                data[baseIndex + 8] = new GeneralSampleValue(sensor.Properties.DistanceRangeBeta);
                
                baseIndex += 9;
            }

            sample.Values = data.ToList();

            return sample;
        }

        public override bool FromGeneralSample(GeneralSample sample)
        {
            SetTime(sample);
            
            if (sample.Protocol == "usound-sample-v1")
            {
                var v = sample.Values.ToArray();
                if (v.Length < 1) return false;
                if (v[0].IsNotNumber()) return false;

                int sensorCount = (int)v[0].number;
                if (v.Length != 1 + sensorCount * 9) return false;

                DataList = new List<USoundSensorData>();
                for (int i = 0; i < sensorCount; i++)
                {
                    var data = new USoundSensorData();
                    data.ID = v[1 + 9 * i].ToInt().Value;
                    data.Distance = v[2 + 9 * i].ToDouble();
                    data.Properties.OriginX = v[3 + 9 * i].ToDouble().Value;
                    data.Properties.OriginY = v[4 + 9 * i].ToDouble().Value;
                    data.Properties.Orientation = v[5 + 9 * i].ToDouble().Value;
                    data.Properties.AngleRangeAlpha = v[6 + 9 * i].ToDouble().Value;
                    data.Properties.AngleRangeBeta = v[7 + 9 * i].ToDouble().Value;
                    data.Properties.DistanceRangeAlpha = v[8 + 9 * i].ToDouble().Value;
                    data.Properties.DistanceRangeBeta = v[9 + 9 * i].ToDouble().Value;
                    DataList.Add(data);
                }

                return true;
            }
            return false;
        }

        public override bool SupportInterpolation()
        {
            return true;
        }

        protected override Sample Interpolate(Sample.SearchResult input)
        {
            var s1 = input.s1 as USoundSample;
            var s2 = input.s2 as USoundSample;
            var w1 = input.w1;
            var w2 = input.w2;

            var output = new USoundSample();
            
            // 遍历第一个样本中的所有传感器数据
            foreach (var sensor1 in s1.DataList)
            {
                // 在第二个样本中查找相同ID的传感器
                foreach (var sensor2 in s2.DataList)
                {
                    // 如果找到相同ID的传感器，则执行插值
                    if (sensor1.ID == sensor2.ID)
                    {
                        var interpolatedSensor = new USoundSensorData();
                        interpolatedSensor.ID = sensor1.ID;
                        
                        // 对于Properties，直接使用第一个样本中的属性
                        // 因为传感器的物理属性通常不会改变
                        interpolatedSensor.Properties = new USoundSensorProperty
                        {
                            OriginX = sensor1.Properties.OriginX,
                            OriginY = sensor1.Properties.OriginY,
                            Orientation = sensor1.Properties.Orientation,
                            AngleRangeAlpha = sensor1.Properties.AngleRangeAlpha,
                            AngleRangeBeta = sensor1.Properties.AngleRangeBeta,
                            DistanceRangeAlpha = sensor1.Properties.DistanceRangeAlpha,
                            DistanceRangeBeta = sensor1.Properties.DistanceRangeBeta
                        };
                        
                        // 对于Distance，只有当两个样本中都有值时才进行插值
                        // 否则设为null
                        if (sensor1.Distance != null && sensor2.Distance != null)
                        {
                            interpolatedSensor.Distance = sensor1.Distance.Value * w1 + sensor2.Distance.Value * w2;
                        }
                        else
                        {
                            interpolatedSensor.Distance = null;
                        }
                        
                        output.DataList.Add(interpolatedSensor);
                        break;
                    }
                }
            }

            return output;
        }
    }
}
